/*
 ------------------------------------------------------------------------------
 Copyright (C) 2006-2007 Team Blur.

 This file is part of the Quantum Engine source code.

 The Quantum Engine source code is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or (at your
 option) any later version.

 The Quantum Engine source code is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 more details.

 You should have received a copy of the GNU General Public License along with
 the Quantum Engine source code; if not, write to the Free Software Foundation, Inc.,
 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 ------------------------------------------------------------------------------
*/


#include "gamelib.h"


odMat3		mat3_identity(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
odMat4		mat4_identity(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);


/*
 ==============================================================================

 3x3 MATRIX

 ==============================================================================
*/


/*
 ==================
 odMat3::ToAngles
 ==================
*/
odAngles odMat3::ToAngles (void) const {

	odAngles	angles;
	float		s, c, theta;

	if (m[0][2] > 1.0f)
		s = 1.0f;
	else if (m[0][2] < -1.0f)
		s = -1.0f;
	else
		s = m[0][2];

	theta = -M_ASin(s);
	c = M_Cos(theta);

	if (c > 8192.0f * M_EPSILON){
		angles.pitch = RAD2DEG(theta);
		angles.yaw = RAD2DEG(M_ATan2(m[0][1], m[0][0]));
		angles.roll = RAD2DEG(M_ATan2(m[1][2], m[2][2]));
	}
	else {
		angles.pitch = RAD2DEG(theta);
		angles.yaw = RAD2DEG(-M_ATan2(m[1][0], m[1][1]));
		angles.roll = 0.0f;
	}

	return angles;
}

/*
 ==================
 odMat3::ToQuat
 ==================
*/
odQuat odMat3::ToQuat (void) const {

	odQuat	quat;
	float	trace, scale, s;
	int		next[3] = {1, 2, 0};
	int		i, j, k;

	trace = m[0][0] + m[1][1] + m[2][2];

	if (trace > 0.0f){
		s = M_Sqrt(trace + 1.0f);
		scale = 0.5f / s;

		quat.x = (m[2][1] - m[1][2]) * scale;
		quat.y = (m[0][2] - m[2][0]) * scale;
		quat.z = (m[1][0] - m[0][1]) * scale;
		quat.w = s * 0.5f;
	}
	else {
		i = 0;
		if (m[1][1] > m[0][0])
			i = 1;
		if (m[2][2] > m[i][i])
			i = 2;

		j = next[i];
		k = next[j];

		s = M_Sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
		scale = 0.5f / s;

		quat[i] = s * 0.5f;
		quat[j] = (m[j][i] + m[i][j]) * scale;
		quat[k] = (m[k][i] + m[i][k]) * scale;
		quat[3] = (m[k][j] - m[j][k]) * scale;
	}

	return quat;
}

/*
 ==================
 odMat3::ToMat4
 ==================
*/
odMat4 odMat3::ToMat4 (void) const {

	odMat4	mat;

	mat[0][0] = m[0][0];
	mat[0][1] = m[0][1];
	mat[0][2] = m[0][2];
	mat[0][3] = 0.0f;
	mat[1][0] = m[1][0];
	mat[1][1] = m[1][1];
	mat[1][2] = m[1][2];
	mat[1][3] = 0.0f;
	mat[2][0] = m[2][0];
	mat[2][1] = m[2][1];
	mat[2][2] = m[2][2];
	mat[2][3] = 0.0f;
	mat[3][0] = 0.0f;
	mat[3][1] = 0.0f;
	mat[3][2] = 0.0f;
	mat[3][3] = 1.0f;

	return mat;
}

/*
 ==================
 odMat3::ToString

 This is just a convenience function for printing matrices
 ==================
*/
const char *odMat3::ToString (int precision) const {

	return Str_FromFloatArray(ToFloatPtr(), 9, precision);
}


/*
 ==============================================================================

 4x4 MATRIX

 ==============================================================================
*/


/*
 ==================
 odMat4::Scale
 ==================
*/
void odMat4::Scale (float x, float y, float z){

	m[0] *= x;
	m[1] *= y;
	m[2] *= z;
}

/*
 ==================
 odMat4::Translate
 ==================
*/
void odMat4::Translate (float x, float y, float z){

	m[3] += m[0] * x + m[1] * y + m[2] * z;
}

/*
 ==================
 odMat4::Shear
 ==================
*/
void odMat4::Shear (float x, float y, float z){

	odVec4	mx, my, mz;

	mx = m[0] * x;
	my = m[1] * y;
	mz = m[2] * z;

	m[0] += my + mz;
	m[1] += mx + mz;
	m[2] += mx + my;
}

/*
 ==================
 odMat4::Rotate
 ==================
*/
void odMat4::Rotate (float angle, float x, float y, float z){

	odVec4	mx, my, mz;
	odVec3	rx, ry, rz;
	float	mag, s, c, inv;
	float	xx, yy, zz, xy, yz, zx, xs, ys, zs;

	mag = M_Sqrt(x*x + y*y + z*z);
	if (mag == 0.0f)
		return;

	mag = 1.0f / mag;

	x *= mag;
	y *= mag;
	z *= mag;

	M_SinCos(DEG2RAD(angle), &s, &c);

	inv = 1.0f - c;

	xx = (x * x) * inv;
	yy = (y * y) * inv;
	zz = (z * z) * inv;
	xy = (x * y) * inv;
	yz = (y * z) * inv;
	zx = (z * x) * inv;

	xs = x * s;
	ys = y * s;
	zs = z * s;

	mx = m[0];
	my = m[1];
	mz = m[2];

	rx.Set(xx + c, xy + zs, zx - ys);
	ry.Set(xy - zs, yy + c, yz + xs);
	rz.Set(zx + ys, yz - xs, zz + c);

	m[0][0] = mx[0] * rx[0] + my[0] * rx[1] + mz[0] * rx[2];
	m[0][1] = mx[1] * rx[0] + my[1] * rx[1] + mz[1] * rx[2];
	m[0][2] = mx[2] * rx[0] + my[2] * rx[1] + mz[2] * rx[2];
	m[0][3] = mx[3] * rx[0] + my[3] * rx[1] + mz[3] * rx[2];
	m[1][0] = mx[0] * ry[0] + my[0] * ry[1] + mz[0] * ry[2];
	m[1][1] = mx[1] * ry[0] + my[1] * ry[1] + mz[1] * ry[2];
	m[1][2] = mx[2] * ry[0] + my[2] * ry[1] + mz[2] * ry[2];
	m[1][3] = mx[3] * ry[0] + my[3] * ry[1] + mz[3] * ry[2];
	m[2][0] = mx[0] * rz[0] + my[0] * rz[1] + mz[0] * rz[2];
	m[2][1] = mx[1] * rz[0] + my[1] * rz[1] + mz[1] * rz[2];
	m[2][2] = mx[2] * rz[0] + my[2] * rz[1] + mz[2] * rz[2];
	m[2][3] = mx[3] * rz[0] + my[3] * rz[1] + mz[3] * rz[2];
}

/*
 ==================
 odMat4::ToMat3
 ==================
*/
odMat3 odMat4::ToMat3 (void) const {

	odMat3	mat;

	mat[0][0] = m[0][0];
	mat[0][1] = m[0][1];
	mat[0][2] = m[0][2];
	mat[1][0] = m[1][0];
	mat[1][1] = m[1][1];
	mat[1][2] = m[1][2];
	mat[2][0] = m[2][0];
	mat[2][1] = m[2][1];
	mat[2][2] = m[2][2];

	return mat;
}

/*
 ==================
 odMat4::ToString

 This is just a convenience function for printing matrices
 ==================
*/
const char *odMat4::ToString (int precision) const {

	return Str_FromFloatArray(ToFloatPtr(), 16, precision);
}
